/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *costmap_2d学习
 *         https://blog.csdn.net/upup_py/article/details/110008695
 *costmap的网格代价计算
costmap是机器人收集传感器信息建立和更新的二维或三维地图。激光雷达或者深度相机作为传感器跑出的2D或3D SLAM地图，都不能直接用于实际的导航，必须将地图转化为costmap,ROS中costmap通常采用grid(网格)形式。栅格地图一个栅格占1个字节，也就是八位，可以存0-255中数据，也就是每个cell cost（网格的值）为0~255。虽然costmap中的每个单元格都可以有255个不同的成本值中的一个，但它使用的基础结构只能表示三个。具体地说，Occupied被占用（有障碍）, Free自由区域（无障碍）, Unknown Space未知区域。

 膨胀是指成本值从占用的单元向外传播的过程，这些值随距离的增加而减小。这里定义了五个与机器人相关的costmap值的相关标志：

(1) Lethal（致命的）:单元中存在实际的障碍物，机器人的中心（center cell）在如果在该网格中，此时机器人必然与障碍物冲突。（代价值为254）

(2) Inscribed（内切）：网格中心位于机器人的内切圆轮廓内，此时机器人也必然与障碍物冲突。（代价值为253）

(3) Possibly circumscribed（可能受限）：网格中心位于机器人的外切圆与内切圆轮廓之间，此时机器人相当于位于障碍物附近，所以不一定冲突，取决于机器人的方位或者说姿态。 使用”可能“一词是因为它可能不是真正的障碍单元，而是一些用户偏好（用户自己规定一些区域，不想让机器人通过），将特定的成本值放入地图中。（代价值为128）

(4) Freespace（自由空间）：网格中心位于机器人外切圆之外，属于没有障碍物的空间。 （代价值为1）

(5) Unknown（未知）：未知的空间。（代价值为0）


 *
 *********************************************************************/
#include <global_planner/planner_core.h>
#include <pluginlib/class_list_macros.hpp>
#include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_2d.h>

#include <global_planner/dijkstra.h>
#include <global_planner/astar.h>
#include <global_planner/grid_path.h>
#include <global_planner/gradient_path.h>
#include <global_planner/quadratic_calculator.h>




/**
 *  * ROS中的全局规划期global_planner功能包，前面已经介绍了nav_core、move_base、costmap_2d功能包。全局规划大都基于静态地图进行规划，产生路径点，然后给到局部规划器进行局部规划，
ROS中常见的全局规划器功能包有navfn、global_planner(Dijkstra、A*）、asr_navfn、Movelt!(常用于机械臂)等.
plan_node文件是全局规划的入口；planner_core是global_planner的核心，进行初始化，调用A*或者Dijkstra进行全局规划；quadratic_calculator（二次逼近方式，常用）
和potential_calculator（直接返回当前点代价最小值，累加前面的代价值）在生成搜索路径中用到；搜索到路径后使用回溯grid_path（栅格路径）、gradient_path（梯度路径）获得最终路径；之后orientation_filter进行方向滤波。
栅格路径：从终点开始找上下左右四个点中最小的栅格直到起点
梯度路径：从周围八个栅格中找到下降梯度最大的点

 */

//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

namespace global_planner {

 //   这段代码实现了一个名为 GlobalPlanner 的类的成员函数 outlineMap()，其作用是将给定的 costarr 数组的边界设置为指定的 value 值。该函数的详细解释如下
/**
 * 具体而言，该函数的实现方式是，分别用指针 pc 指向数组的第一行、最后一行、第一列和最后一列，
 * 然后使用循环依次遍历这四个边界，并将它们的每个元素设置为指定的 value 值。

在代码的实现过程中，nx 和 ny 分别表示数组 costarr 的宽度和高度，unsigned char 是一个无符号字符类型，用
 于存储 8 位无符号整数。因此，该函数的输入参数包括一个指向无符号字符数组的指针 costarr，数组的宽度 nx 和高度 ny，以及要设置的值 value。
 * @param costarr
 * @param nx
 * @param ny
 * @param value
 */
void GlobalPlanner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) {
    // 首先，将数组第一行全部设置为指定的值
    unsigned char* pc = costarr;
    for (int i = 0; i < nx; i++)
        *pc++ = value;
    // 接着，将数组最后一行全部设置为指定的值
    pc = costarr + (ny - 1) * nx;
    for (int i = 0; i < nx; i++)
        *pc++ = value;
    //  // 然后，将数组第一列全部设置为指定的值
    pc = costarr;
    for (int i = 0; i < ny; i++, pc += nx)
        *pc = value;

    // // 最后，将数组最后一列全部设置为指定的值
    pc = costarr + nx - 1;
    for (int i = 0; i < ny; i++, pc += nx)
        *pc = value;

         /**
        pc = costarr + (ny - 1) * nx; 解释一下
        这行代码的作用是让指针 pc 指向数组 costarr 的最后一行的第一个元素。具体而言，由于 costarr 是一个一维数组，按照行优先的顺序存储，因此最后一行的第一个元素的下标应该是 (ny - 1) * nx。
        这里用指针的算术运算 costarr + (ny - 1) * nx 来获得该元素的地址，并将其赋值给指针 pc。这样，接下来就可以通过对 pc 进行递增操作，遍历数组 costarr 的最后一行了。
        */
}

GlobalPlanner::GlobalPlanner() :
        costmap_(NULL), initialized_(false), allow_unknown_(true),
        p_calc_(NULL), planner_(NULL), path_maker_(NULL), orientation_filter_(NULL),
        potential_array_(NULL) {
}

GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
        GlobalPlanner() {
    //initialize the planner
    initialize(name, costmap, frame_id);
}

GlobalPlanner::~GlobalPlanner() {
    if (p_calc_)
        delete p_calc_;
    if (planner_)
        delete planner_;
    if (path_maker_)
        delete path_maker_;
    if (dsrv_)
        delete dsrv_;
}

/**
 * ROS中的全局规划期global_planner功能包，前面已经介绍了nav_core、move_base、costmap_2d功能包。全局规划大都基于静态地图进行规划，产生路径点，然后给到局部规划器进行局部规划，
ROS中常见的全局规划器功能包有navfn、global_planner(Dijkstra、A*）、asr_navfn、Movelt!(常用于机械臂)等.
plan_node文件是全局规划的入口；planner_core是global_planner的核心，进行初始化，调用A*或者Dijkstra进行全局规划；quadratic_calculator（二次逼近方式，常用）
和potential_calculator（直接返回当前点代价最小值，累加前面的代价值）在生成搜索路径中用到；搜索到路径后使用回溯grid_path（栅格路径）、gradient_path（梯度路径）获得最终路径；之后orientation_filter进行方向滤波。
栅格路径：从终点开始找上下左右四个点中最小的栅格直到起点
梯度路径：从周围八个栅格中找到下降梯度最大的点
 * @param name
 * @param costmap
 * @param frame_id
 */

///home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/move_base/src/move_base.cpp  全局路经规划初始化 172
/// planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);//全局路经规划初始化 172
 // /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/move_base/src/move_base.cpp 166
 //   planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);//全局规划地图 166
 //   planner_costmap_ros_->pause();//暂停全局代价地图的更新，但是不会关闭传感器话题的订阅
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {
    initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
}


/**
 * 进行初始化：如果没有初始化，就执行初始化操作，获取costmap的尺寸大小，初始化参数：old_navfn_behavior_、use_quadratic（是否使用二次逼近：是：调用QuadraticCalculator-推荐，否：调用PotentialCalculator）、use_dijkstra（true:使用dijkstra,false:A*）、use_grid_path（是：栅格路径；否：梯度路径–一般平滑性较好）

 * @param name
 * @param costmap
 * @param frame_id
 */
//140行
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) {
    if (!initialized_) {
        ros::NodeHandle private_nh("~/" + name);
        costmap_ = costmap;
        frame_id_ = frame_id;
         //地图的大小，即像素数。
        unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
//参数：判断使用的是navfn包还是，old_navfn_behavior_这个参数默认为false，用来兼容navfn；然后调用clearEndpoint（）函数，该函数把终点周围的点更新了一下：
    //    old_navfn_behavior(default: false): 若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.

        private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
        if(!old_navfn_behavior_)
            convert_offset_ = 0.5;//false
        else
            convert_offset_ = 0.0;//true

        bool use_quadratic;
        private_nh.param("use_quadratic", use_quadratic, true);//是否二次逼近获取路径
        if (use_quadratic) //use_quadratic（是否使用二次逼近：是：调用QuadraticCalculator-推荐，否：调用PotentialCalculator）
            p_calc_ = new QuadraticCalculator(cx, cy);//   // cx, cy 地图的大小，即像素数。  实现了 PotentialCalculator接口
        else
            p_calc_ = new PotentialCalculator(cx, cy);

        bool use_dijkstra;// 用a* 还是 dijkstra 做全局规划
        private_nh.param("use_dijkstra", use_dijkstra, true);
        if (use_dijkstra)
        {
            //// cx, cy 地图的大小，即像素数。
            DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);//true
            if(!old_navfn_behavior_)
                de->setPreciseStart(true);
            planner_ = de;
        }
        else
            planner_ = new AStarExpansion(p_calc_, cx, cy);//false  // cx, cy 地图的大小，即像素数。

        bool use_grid_path;//是否使用栅格路径  如果如果设置为true,则会规划一条沿着网格边界的路径,偏向于直线穿越网格,否则将使用梯度下降算法,路径更为光滑点.
        private_nh.param("use_grid_path", use_grid_path, false);
        if (use_grid_path)
            path_maker_ = new GridPath(p_calc_);//new 出path_maker_实例，从可行点中提取路径
        else
            path_maker_ = new GradientPath(p_calc_);//梯度下降算法,路径更为光滑点
        ////方向滤波
        orientation_filter_ = new OrientationFilter();//进行方向滤波
//路径发布
        plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
        //发布代价地图
        potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
//是否探索未知区域，flase--不可到达
        private_nh.param("allow_unknown", allow_unknown_, true);
        planner_->setHasUnknown(allow_unknown_);
        //窗口信息
        private_nh.param("planner_window_x", planner_window_x_, 0.0);
        private_nh.param("planner_window_y", planner_window_y_, 0.0);
        private_nh.param("default_tolerance", default_tolerance_, 0.0);//#默认0.0，目标容差
        private_nh.param("publish_scale", publish_scale_, 100);
        private_nh.param("outline_map", outline_map_, true);
        private_nh.param("isBezierLine", isBezierLine_, false);

        make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

        dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
        dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb =
                [this](auto& config, auto level){ reconfigureCB(config, level); };
        dsrv_->setCallback(cb);
                //sukai
        ros::NodeHandle node;
        //2.恢复地图,清除地图,去除小车initpose后留下的残影
       updateCleancostmap_Pub = node.advertise<std_msgs::String>("/updateCleancostmap", 10);
        initialized_ = true;
    } else
        ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");

}

void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig& config, uint32_t level) {
    if(isreconfigureFirst_){
        config.isBezierLine=isBezierLine_;
        isreconfigureFirst_=false;
    }
    planner_->setLethalCost(config.lethal_cost);
    path_maker_->setLethalCost(config.lethal_cost);
    planner_->setNeutralCost(config.neutral_cost);
    planner_->setFactor(config.cost_factor);
    publish_potential_ = config.publish_potential;
    orientation_filter_->setMode(config.orientation_mode);
    orientation_filter_->setWindowSize(config.orientation_window_size);
    //sukai
    isBezierLine_=config.isBezierLine;
}
    //清除成本图中的起始单元格，因为我们知道它不会成为障碍
void GlobalPlanner::clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my) {
    if (!initialized_) {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return;
    }

    //set the associated costs in the cost map to be free
    costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);//自由空间
}

bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) {
    makePlan(req.start, req.goal, resp.plan.poses);

    resp.plan.header.stamp = ros::Time::now();
    resp.plan.header.frame_id = frame_id_;

    return true;
}

void GlobalPlanner::mapToWorld(double mx, double my, double& wx, double& wy) {
    wx = costmap_->getOriginX() + (mx+convert_offset_) * costmap_->getResolution();
    wy = costmap_->getOriginY() + (my+convert_offset_) * costmap_->getResolution();
}

/**
 *
 * @param wx
 * @param wy
 * @param mx
 * @param my
 这个函数的实现比较简单，主要包括以下几个步骤：

    检查给定的世界坐标 (wx, wy) 是否在 Costmap2D 对象覆盖的区域内，如果不在，则返回 false。

    将给定的世界坐标 (wx, wy) 转换为相应的离散地图坐标 (mx, my)。

    检查转换后的地图坐标 (mx, my) 是否在 Costmap2D 对象的有效范围内，如果超出，则返回 false。

    如果以上检查都通过了，则返回 true。

下面是对这个函数中各个参数的说明：

    wx 和 wy：待转换的世界坐标。
    mx 和 my：转换后的离散地图坐标。像素坐标
    origin_x_ 和 origin_y_：地图坐标系原点在世界坐标系中的位置。
    resolution_：地图的分辨率，即一个像素代表的实际距离。
    size_x_ 和 size_y_：地图的大小，即像素数。

需要注意的是，以上实现中使用了 C++ 中的类型转换语句 static_cast，将浮点数转换为整数类型，同时使用引用类型 & 将结果返回给调用者。
 */

bool GlobalPlanner::worldToMap(double wx, double wy, double& mx, double& my) {
    double origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY();
    double resolution = costmap_->getResolution();

    if (wx < origin_x || wy < origin_y)
        return false;

    /**
     * //参数：判断使用的是navfn包还是，old_navfn_behavior_这个参数默认为false，用来兼容navfn；然后调用clearEndpoint（）函数，该函数把终点周围的点更新了一下：
    //    old_navfn_behavior(default: false): 若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.

        if(!old_navfn_behavior_)
            convert_offset_ = 0.5;//false
        else
            convert_offset_ = 0.0;//true
    */
    mx = (wx - origin_x) / resolution - convert_offset_;
    my = (wy - origin_y) / resolution - convert_offset_;
   //  costmap_->getSizeInCellsX() 和 getSizeInCellsY：地图的大小，即像素数。
    if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY())
        return true;

    return false;
}


//todo 1.规划路径
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           std::vector<geometry_msgs::PoseStamped>& plan) {
    return makePlan(start, goal, default_tolerance_, plan);
}



/**
  * create Bezier
  * sukai
  */

//一阶 Function to return point on Linear Bezier Curve
//ps:起始点   pe:结束点  t: 0-1之间的数据；t:   绘制40个点，i从0开始计数到最大值40，计算  t = i / 40.0
std::vector<double> GlobalPlanner::FirstBezier(std::vector<double> ps, std::vector<double> pe, double t)
// vector<double> GlobalPlanner::FirstBezier(vector<double> pe, vector<double> ps, double t)
{
    std::vector<double> point;
    point.push_back((1-t)*ps[0]+t*pe[0]);
    point.push_back((1-t)*ps[1]+t*pe[1]);
//        cout << "FirstBezier ps[0]:pe[0] " << to_string(ps[0]) << ", " << to_string(pe[0])  << endl;
//        cout << "FirstBezier ps[1]:pe[1] " << to_string(ps[1]) << ", " << to_string(pe[1])  << endl;
//        cout << "FirstBezier t "  << to_string(t)  << endl;
//        cout << "FirstBezier point[0]:point[1] " << to_string(point[0]) << ", " << to_string(point[1])  << endl;

    return point;
}
    //计算起点在一维数组中的下标。// 将起点坐标 start_x 和 start_y 转化为一维数组中的索引 start_i。
int GlobalPlanner::toIndex(int x, int y,int nx ,int ny) {
        return x + nx * y;
    }
//检查代代价值非自由空间;Freespace（自由空间）：网格中心位于机器人外切圆之外，属于没有障碍物的空间。 （代价值为1）;https://blog.csdn.net/upup_py/article/details/110008695
bool GlobalPlanner::checkCost(const std::vector<int>& indexs) {
    for (int index : indexs) {
        char cost = getCharMapValue(index);//获取代价值
       // std::cout << "0.Cost at index " << index << " is " << cost << std::endl;
        if (static_cast<int>(cost) != 0 ) { // Assuming '1' represents free space
          //  std::cout << "1. Cost at index " << index << " is " << static_cast<int>(cost) << ". It is not free space." << std::endl;
            return false;
        }
       // std::cout << "2.Cost at index " << index << " is " << cost << std::endl;
    }
    return true;
}

  //  ROS_ERROR("getBezierLine 通过开始点与控制点计算两点之间的像素直,路径太短,退出贝塞尔规划,使用a*算法.index is %i, cost is %s", cost);
//获取代价值
char GlobalPlanner::getCharMapValue(int index) {

        int width = costmap_->getSizeInCellsX(); // 获取代价地图的宽度（以单元格为单位）
        int height = costmap_->getSizeInCellsY(); // 获取代价地图的高度（以单元格为单位）
        int map_size = width * height; // 计算二维数组的总长度


        if (index >= 0 && index < map_size) {
        return costmap_->getCharMap()[index];
    } else {
        // Return a default value indicating out-of-bounds or invalid index
        return 'X'; // You can choose any appropriate default value
    }
}

    //打印代价地图数据
    void GlobalPlanner::printCostMap() {

        //打印代价地图数据
        int width =        costmap_->getSizeInCellsX();
        int height =       costmap_->getSizeInCellsY();
        for (int y = 0; y < height; ++y) {
            for (int x = 0; x < width; ++x) {
                int index = y * width + x;
                unsigned char cost = costmap_->getCharMap()[index];
               // std::cout << "Cost at (" << x << ", " << y << ") is " << static_cast<int>(cost) << std::endl;
            }
        }

    }





    //todo 通过起始点和结束点获取一条 贝赛尔直线 路径; startPoseStamped:起始点 endPoseStamped:结束点 plan:路径 (double)
bool  GlobalPlanner::getBezierLine(double start_x,double start_y,double goal_x,double goal_y,std::vector<geometry_msgs::PoseStamped> &plan){
        //打印代价地图数据
       // printCostMap();



        int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
        std::string global_frame = frame_id_;
        //clear the plan, just in case
        plan.clear();
        std::vector<std::pair<float, float> > path;

        std::vector<double> ps;   //起始点
        std::vector<double> pe;   //结束点
        ps.push_back(start_x);
        ps.push_back(start_y);
        pe.push_back(goal_x);
        pe.push_back(goal_y);

        std::vector<int> indexs;   //路径索引
        int start_index = toIndex(start_x, start_y,nx,ny);//将周围点的坐标转化为数组下标。
        indexs.push_back(start_index);

//        //定义一个pair类型的变量 current，用于存储当前点的坐标。
//        std::pair<double, double> current;
//        current.first = goal_x;//将终点的x坐标赋值给current的first成员。
//        current.second = goal_y;//将终点的y坐标赋值给current的second成员。
//        path.push_back(current);//将终点加入路径中。
        std::pair<double, double> current_start;
        //起始点加入path集合中中
        current_start.first = start_x;//赋值给current的first成员。
        current_start.second = start_y;//赋值给current的second成员。
        path.push_back(current_start);//将终点加入路径中。

        int  distance =sqrt(pow((int)goal_x - start_x, 2) + pow((int)goal_y - start_y, 2));
       // ROS_INFO("getBezierLine 通过开始点与控制点计算两点直接的像素直 distance is %i", distance);

        // rostopic echo /move_base/GlobalPlanner/plan

        for (int i = 1; i < distance; i++)
        {
            double t =(double)  i / (double) distance;
            //todo ************************** 一阶贝塞尔曲线计算插值 *********** ========================================================*****************
            std::vector<double> result = FirstBezier(ps, pe,  t);
            current_start.first = result[0];//赋值给current的first成员。
            current_start.second = result[1];//赋值给current的second成员。
            path.push_back(current_start);//将终点加入路径中。
            int index = toIndex(result[0], result[1],nx,ny);//将周围点的坐标转化为数组下标。
            indexs.push_back(index);
        }

//        if(indexs.size()<=2){
//            ROS_ERROR("getBezierLine 通过开始点与控制点计算两点之间的像素直,路径太短,退出贝塞尔规划,使用a*算法. distance is %i", distance);
//            return false;
//        }

        //目标点加入index集合中
        int goal_index = toIndex(goal_x, goal_y,nx,ny);
        indexs.push_back(goal_index);



        //定义一个pair类型的变量 current，用于存储当前点的坐标。
        std::pair<double, double> current_goal;
        current_goal.first = goal_x;//将终点的x坐标赋值给current的first成员。
        current_goal.second = goal_y;//将终点的y坐标赋值给current的second成员。
        path.push_back(current_goal);//将终点加入路径中。

        //检查路径中是否有障碍物
        bool checkCostFig =  checkCost(indexs);
        if(!checkCostFig){
          //  ROS_ERROR("getBezierLine 通过开始点与控制点计算两点之间的像素直,路径中有障碍物,退出贝塞尔规划,使用a*算法. distance is %i", distance);
            return checkCostFig;
        }

        ros::Time plan_time = ros::Time::now();
        //for (int i = path.size() -1; i>=0; i--) {
            for (int i = 0; i<path.size(); i++) {
            std::pair<float, float> point = path[i];
            //convert the plan to world coordinates
            double world_x, world_y;
            //像素坐标转世界坐标
            mapToWorld(point.first, point.second, world_x, world_y);

            geometry_msgs::PoseStamped pose;
            pose.header.stamp = plan_time;
            pose.header.frame_id = global_frame;
            pose.pose.position.x = world_x;
            pose.pose.position.y = world_y;
            pose.pose.position.z = 0.0;
            pose.pose.orientation.x = 0.0;
            pose.pose.orientation.y = 0.0;
            pose.pose.orientation.z = 0.0;
            pose.pose.orientation.w = 1.0;
            plan.push_back(pose);
        }

        return !plan.empty();

    }

//todo 2.规划路径
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
    boost::mutex::scoped_lock lock(mutex_);
    if (!initialized_) {
        //此计划器尚未初始化，但正在使用中，请在使用前调用initialize（）
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return false;
    }

    //clear the plan, just in case  清除计划，以防万一
    plan.clear();

    ros::NodeHandle n;
    std::string global_frame = frame_id_;
       //直到tf能够处理改变过去的事情。。。我们将要求目标在我们的全球框架内
    //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
    if (goal.header.frame_id != global_frame) {
        ROS_ERROR(
                "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
        return false;
    }

    if (start.header.frame_id != global_frame) {
        ROS_ERROR(
                "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
        return false;
    }

    double wx = start.pose.position.x;
    double wy = start.pose.position.y;

    unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
    double start_x, start_y, goal_x, goal_y;


   //世界坐标转像素坐标  double wx = start.pose.position.x;wy = start.pose.position.y;
    if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {//世界坐标转像素坐标
    //机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗？
        ROS_WARN(
                "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
        return false;
    }

        //参数：判断使用的是navfn包还是，old_navfn_behavior_这个参数默认为false，用来兼容navfn；然后调用clearEndpoint（）函数，该函数把终点周围的点更新了一下：
        //    old_navfn_behavior(default: false): 若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.

        if(old_navfn_behavior_){
        start_x = start_x_i;
        start_y = start_y_i;
    }else{
            //起点 世界坐标转像素坐标  double wx = start.pose.position.x;wy = start.pose.position.y;
        worldToMap(wx, wy, start_x, start_y);//世界坐标转像素坐标
    }

    wx = goal.pose.position.x;
    wy = goal.pose.position.y;

    if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
        ROS_WARN_THROTTLE(1.0,
                "The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
        return false;
    }
    if(old_navfn_behavior_){
        goal_x = goal_x_i;
        goal_y = goal_y_i;
    }else{
        //目标点 世界坐标转像素坐标  double wx = goal.pose.position.x;wy = goal.pose.position.y;
        worldToMap(wx, wy, goal_x, goal_y);
    }

    //clear the starting cell within the costmap because we know it can't be an obstacle 清除成本图中的起始单元格，因为我们知道它不会成为障碍
    //清除成本图中的起始单元格，因为我们知道它不会成为障碍
    clearRobotCell(start, start_x_i, start_y_i);
    //nx 和 ny 分别表示数组 地图 的宽度和高度
    int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();

    //make sure to resize the underlying array that Navfn uses 确保调整Navfn使用的基础数组的大小
    p_calc_->setSize(nx, ny);
    planner_->setSize(nx, ny);
    path_maker_->setSize(nx, ny);
    potential_array_ = new float[nx * ny];





    if(outline_map_)
        outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);  //costmap_2d::LETHAL_OBSTACLE = 254表示在此单元为会发生碰撞的障碍物。




//todo  sukai
bool  getBezierLinefig=false;
if(isBezierLine_){

    getBezierLinefig  =   getBezierLine( start_x, start_y, goal_x, goal_y,plan);

}

//走a*
if(!isBezierLine_||!getBezierLinefig){


                bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                                 nx * ny * 2, potential_array_);//float* potential_array_; 该函数的作用是计算出从起点到终点的所有路径的代价值，即potential_array_数组

                if(!old_navfn_behavior_)
                    planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);



         

            if(publish_potential_)
                publishPotential(potential_array_);

            if (found_legal) {

                if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {

                    geometry_msgs::PoseStamped goal_copy = goal;
                    goal_copy.header.stamp = ros::Time::now();
                    plan.push_back(goal_copy);
                } else {
                    ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");

                   
                    std_msgs::String data;
                    updateCleancostmap_Pub.publish(data);

                }
            }else{
                ROS_ERROR_THROTTLE(5.0, "Failed to get a plan.");//无法获取计划
              
                std_msgs::String data;
                updateCleancostmap_Pub.publish(data);
            }



    }
        // add orientations if needed 如果需要，添加方向
        orientation_filter_->processPath(start, plan);
        if(isBezierLine_&&getBezierLinefig){
            // 替换 plan 向量中的最后一个元素为 goal
            if (!plan.empty()) {  // 确保向量非空
                plan.back() = goal;
            }
        }

        //publish the plan for visualization purposes 出于可视化目的发布计划
        publishPlan(plan);
        delete[] potential_array_;
        return !plan.empty();
}



    //出于可视化目的发布计划
    void GlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
        if (!initialized_) {
            ROS_ERROR(
                    "This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return;
        }

        //create a message for the plan
        nav_msgs::Path gui_path;
        gui_path.poses.resize(path.size());

        gui_path.header.frame_id = frame_id_;
        gui_path.header.stamp = ros::Time::now();

        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
        for (unsigned int i = 0; i < path.size(); i++) {
            gui_path.poses[i] = path[i];
        }

        plan_pub_.publish(gui_path);
    }

    bool GlobalPlanner::getPlanFromPotential(double start_x, double start_y, double goal_x, double goal_y,
                                          const geometry_msgs::PoseStamped& goal,
                                           std::vector<geometry_msgs::PoseStamped>& plan) {
        if (!initialized_) {
            ROS_ERROR(
                    "This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return false;
        }

        std::string global_frame = frame_id_;

        //clear the plan, just in case
        plan.clear();

        std::vector<std::pair<float, float> > path;
             // float* potential_array_;   //potential（预估每个点到目标节点的距离）
             //191  path_maker_ = new GridPath(p_calc_);//new 出path_maker_实例，从可行点中提取路径  ||  path_maker_ = new GradientPath(p_calc_);//梯度下降算法,路径更为光滑点
        if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)) {//start_x, start_y, goal_x, goal_y 像素坐标
            ROS_ERROR("NO PATH!");
            return false;
        }

        ros::Time plan_time = ros::Time::now();
        for (int i = path.size() -1; i>=0; i--) {
            std::pair<float, float> point = path[i];
            //convert the plan to world coordinates
            double world_x, world_y;
            ////像素坐标转世界坐标
            mapToWorld(point.first, point.second, world_x, world_y);

            geometry_msgs::PoseStamped pose;
            pose.header.stamp = plan_time;
            pose.header.frame_id = global_frame;
            pose.pose.position.x = world_x;
            pose.pose.position.y = world_y;
            pose.pose.position.z = 0.0;
            pose.pose.orientation.x = 0.0;
            pose.pose.orientation.y = 0.0;
            pose.pose.orientation.z = 0.0;
            pose.pose.orientation.w = 1.0;
            plan.push_back(pose);
        }
        if(old_navfn_behavior_){
                plan.push_back(goal);
        }
        return !plan.empty();
    }
//potential（预估每个点到目标节点的距离） 发布潜力    potential_array_ 潜力数组
/**
 * 这个函数是一个全局路径规划器的类中的一个函数，用于发布潜在代价地图。函数的输入是一个浮点型数组，表示每个点到目标节点的距离。函数的输出是一个nav_msgs::OccupancyGrid类型的消息，表示整个代价地图。
 * 函数首先获取代价地图的大小和分辨率，
 * 然后将整个代价地图发布出去。在发布代价地图之前，函数会对代价地图进行处理，将代价地图中的每个点的值进行归一化处理，使得代价地图中的最大值为1，最小值为0。最后，函数将处理后的代价地图发布出去。
 * @param potential
 */
    void GlobalPlanner::publishPotential(float* potential)
    {
        int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
        double resolution = costmap_->getResolution();
        nav_msgs::OccupancyGrid grid;
        // Publish Whole Grid
        grid.header.frame_id = frame_id_;
        grid.header.stamp = ros::Time::now();
        grid.info.resolution = resolution;

        grid.info.width = nx;
        grid.info.height = ny;

        double wx, wy;
        costmap_->mapToWorld(0, 0, wx, wy);
        grid.info.origin.position.x = wx - resolution / 2;
        grid.info.origin.position.y = wy - resolution / 2;
        grid.info.origin.position.z = 0.0;
        grid.info.origin.orientation.w = 1.0;

        grid.data.resize(nx * ny);

        float max = 0.0;
        for (unsigned int i = 0; i < grid.data.size(); i++) {
            float potential = potential_array_[i];
            if (potential < POT_HIGH) {//potential（预估每个点到目标节点的距离） 值
                if (potential > max) {
                    max = potential;
                }
            }
        }

        for (unsigned int i = 0; i < grid.data.size(); i++) {
            if (potential_array_[i] >= POT_HIGH) {
                grid.data[i] = -1;
            } else {
                if (fabs(max) < DBL_EPSILON) {
                    grid.data[i] = -1;
                } else {
                    grid.data[i] = potential_array_[i] * publish_scale_ / max;
                }
            }
        }
        potential_pub_.publish(grid);
    }

    } //end namespace global_planner

